home *** CD-ROM | disk | FTP | other *** search
/ PC PowerPlay 58 / pcpp58a.iso / extras / quake 3 source / Q3A_ToolSource.exe / Main / MATHLIB.CPP < prev    next >
Encoding:
C/C++ Source or Header  |  2001-01-02  |  5.4 KB  |  291 lines
  1. // mathlib.c -- math primitives
  2.  
  3. #include "stdafx.h"
  4. #include "cmdlib.h"
  5. #include "mathlib.h"
  6.  
  7. vec3_t vec3_origin = {0.0f,0.0f,0.0f};
  8.  
  9.  
  10. float VectorLength(vec3_t v)
  11. {
  12.     int        i;
  13.     float    length;
  14.     
  15.     length = 0.0f;
  16.     for (i=0 ; i< 3 ; i++)
  17.         length += v[i]*v[i];
  18.     length = (float)sqrt (length);
  19.  
  20.     return length;
  21. }
  22.  
  23. qboolean VectorCompare (vec3_t v1, vec3_t v2)
  24. {
  25.     int        i;
  26.     
  27.     for (i=0 ; i<3 ; i++)
  28.         if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON)
  29.             return false;
  30.             
  31.     return true;
  32. }
  33.  
  34. vec_t Q_rint (vec_t in)
  35. {
  36.   if (g_PrefsDlg.m_bNoClamp)
  37.     return in;
  38.   else
  39.       return (float)floor (in + 0.5);
  40. }
  41.  
  42. void VectorMA (vec3_t va, float scale, vec3_t vb, vec3_t vc)
  43. {
  44.     vc[0] = va[0] + scale*vb[0];
  45.     vc[1] = va[1] + scale*vb[1];
  46.     vc[2] = va[2] + scale*vb[2];
  47. }
  48.  
  49. void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
  50. {
  51.     cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
  52.     cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
  53.     cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
  54. }
  55.  
  56. vec_t _DotProduct (vec3_t v1, vec3_t v2)
  57. {
  58.     return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
  59. }
  60.  
  61. void _VectorSubtract (vec3_t va, vec3_t vb, vec3_t out)
  62. {
  63.     out[0] = va[0]-vb[0];
  64.     out[1] = va[1]-vb[1];
  65.     out[2] = va[2]-vb[2];
  66. }
  67.  
  68. void _VectorAdd (vec3_t va, vec3_t vb, vec3_t out)
  69. {
  70.     out[0] = va[0]+vb[0];
  71.     out[1] = va[1]+vb[1];
  72.     out[2] = va[2]+vb[2];
  73. }
  74.  
  75. void _VectorCopy (vec3_t in, vec3_t out)
  76. {
  77.     out[0] = in[0];
  78.     out[1] = in[1];
  79.     out[2] = in[2];
  80. }
  81.  
  82. vec_t VectorNormalize (vec3_t v)
  83. {
  84.     int        i;
  85.     float    length;
  86.     
  87.     length = 0.0f;
  88.     for (i=0 ; i< 3 ; i++)
  89.         length += v[i]*v[i];
  90.     length = (float)sqrt (length);
  91.     if (length == 0)
  92.         return (vec_t)0;
  93.         
  94.     for (i=0 ; i< 3 ; i++)
  95.         v[i] /= length;    
  96.  
  97.     return length;
  98. }
  99.  
  100. void VectorInverse (vec3_t v)
  101. {
  102.     v[0] = -v[0];
  103.     v[1] = -v[1];
  104.     v[2] = -v[2];
  105. }
  106.  
  107. void VectorScale (vec3_t v, vec_t scale, vec3_t out)
  108. {
  109.     out[0] = v[0] * scale;
  110.     out[1] = v[1] * scale;
  111.     out[2] = v[2] * scale;
  112. }
  113.  
  114.  
  115. void VectorRotate (vec3_t vIn, vec3_t vRotation, vec3_t out)
  116. {
  117.   vec3_t vWork, va;
  118.   VectorCopy(vIn, va);
  119.   VectorCopy(va, vWork);
  120.   int nIndex[3][2];
  121.   nIndex[0][0] = 1; nIndex[0][1] = 2;
  122.   nIndex[1][0] = 2; nIndex[1][1] = 0;
  123.   nIndex[2][0] = 0; nIndex[2][1] = 1;
  124.  
  125.   for (int i = 0; i < 3; i++)
  126.   {
  127.     if (vRotation[i] != 0)
  128.     {
  129.       double dAngle = vRotation[i] * Q_PI / 180.0;
  130.       double c = cos(dAngle);
  131.       double s = sin(dAngle);
  132.       vWork[nIndex[i][0]] = va[nIndex[i][0]] * c - va[nIndex[i][1]] * s;
  133.       vWork[nIndex[i][1]] = va[nIndex[i][0]] * s + va[nIndex[i][1]] * c;
  134.     }
  135.     VectorCopy(vWork, va);
  136.   }
  137.   VectorCopy(vWork, out);
  138. }
  139.  
  140. void VectorRotate (vec3_t vIn, vec3_t vRotation, vec3_t vOrigin, vec3_t out)
  141. {
  142.   vec3_t vTemp, vTemp2;
  143.   VectorSubtract(vIn, vOrigin, vTemp);
  144.   VectorRotate(vTemp, vRotation, vTemp2);
  145.   VectorAdd(vTemp2, vOrigin, out);
  146. }
  147.  
  148. void VectorPolar(vec3_t v, float radius, float theta, float phi)
  149. {
  150.      v[0]=float(radius * cos(theta) * cos(phi));
  151.     v[1]=float(radius * sin(theta) * cos(phi));
  152.     v[2]=float(radius * sin(phi));
  153. }
  154.  
  155. void VectorSnap(vec3_t v)
  156. {
  157.   for (int i = 0; i < 3; i++)
  158.   {
  159.     v[i] = floor (v[i] + 0.5);
  160.   }
  161. }
  162.  
  163.  
  164. void _Vector5Add (vec5_t va, vec5_t vb, vec5_t out)
  165. {
  166.     out[0] = va[0]+vb[0];
  167.     out[1] = va[1]+vb[1];
  168.     out[2] = va[2]+vb[2];
  169.     out[3] = va[3]+vb[3];
  170.     out[4] = va[4]+vb[4];
  171. }
  172.  
  173. void _Vector5Scale (vec5_t v, vec_t scale, vec5_t out)
  174. {
  175.     out[0] = v[0] * scale;
  176.     out[1] = v[1] * scale;
  177.     out[2] = v[2] * scale;
  178.     out[3] = v[3] * scale;
  179.     out[4] = v[4] * scale;
  180. }
  181.  
  182. void _Vector53Copy (vec5_t in, vec3_t out)
  183. {
  184.     out[0] = in[0];
  185.     out[1] = in[1];
  186.     out[2] = in[2];
  187. }
  188.  
  189. // NOTE: added these from Ritual's Q3Radiant
  190. void ClearBounds (vec3_t mins, vec3_t maxs)
  191. {
  192.     mins[0] = mins[1] = mins[2] = 99999;
  193.     maxs[0] = maxs[1] = maxs[2] = -99999;
  194. }
  195.  
  196. void AddPointToBounds (vec3_t v, vec3_t mins, vec3_t maxs)
  197. {
  198.     int        i;
  199.     vec_t    val;
  200.     
  201.     for (i=0 ; i<3 ; i++)
  202.     {
  203.         val = v[i];
  204.         if (val < mins[i])
  205.             mins[i] = val;
  206.         if (val > maxs[i])
  207.             maxs[i] = val;
  208.     }
  209. }
  210.  
  211. #define    PITCH                0        // up / down
  212. #define    YAW                    1        // left / right
  213. #define    ROLL                2        // fall over
  214. #ifndef M_PI
  215. #define M_PI        3.14159265358979323846    // matches value in gcc v2 math.h
  216. #endif
  217.  
  218. void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
  219. {
  220.     float        angle;
  221.     static float        sr, sp, sy, cr, cp, cy;
  222.     // static to help MS compiler fp bugs
  223.     
  224.     angle = angles[YAW] * (M_PI*2 / 360);
  225.     sy = sin(angle);
  226.     cy = cos(angle);
  227.     angle = angles[PITCH] * (M_PI*2 / 360);
  228.     sp = sin(angle);
  229.     cp = cos(angle);
  230.     angle = angles[ROLL] * (M_PI*2 / 360);
  231.     sr = sin(angle);
  232.     cr = cos(angle);
  233.     
  234.     if (forward)
  235.     {
  236.         forward[0] = cp*cy;
  237.         forward[1] = cp*sy;
  238.         forward[2] = -sp;
  239.     }
  240.     if (right)
  241.     {
  242.         right[0] = -sr*sp*cy+cr*sy;
  243.         right[1] = -sr*sp*sy-cr*cy;
  244.         right[2] = -sr*cp;
  245.     }
  246.     if (up)
  247.     {
  248.         up[0] = cr*sp*cy+sr*sy;
  249.         up[1] = cr*sp*sy-sr*cy;
  250.         up[2] = cr*cp;
  251.     }
  252. }
  253.  
  254. void VectorToAngles( vec3_t vec, vec3_t angles )
  255. {
  256.     float forward;
  257.     float yaw, pitch;
  258.     
  259.     if ( ( vec[ 0 ] == 0 ) && ( vec[ 1 ] == 0 ) )
  260.     {
  261.         yaw = 0;
  262.         if ( vec[ 2 ] > 0 )
  263.         {
  264.             pitch = 90;
  265.         }
  266.         else
  267.         {
  268.             pitch = 270;
  269.         }
  270.     }
  271.     else
  272.     {
  273.         yaw = atan2( vec[ 1 ], vec[ 0 ] ) * 180 / M_PI;
  274.         if ( yaw < 0 )
  275.         {
  276.             yaw += 360;
  277.         }
  278.         
  279.         forward = ( float )sqrt( vec[ 0 ] * vec[ 0 ] + vec[ 1 ] * vec[ 1 ] );
  280.         pitch = atan2( vec[ 2 ], forward ) * 180 / M_PI;
  281.         if ( pitch < 0 )
  282.         {
  283.             pitch += 360;
  284.         }
  285.     }
  286.     
  287.     angles[ 0 ] = pitch;
  288.     angles[ 1 ] = yaw;
  289.     angles[ 2 ] = 0;
  290. }
  291.